/*
 * ekf_uwb.cpp
 *
 *  Created on: 2023年6月25日
 *      Author: CJP
 */

#include "ekf_uwb_yaw/ekf_uwb_yaw.hpp"

void EKFUWB::EkfUwb_Init()
{
	Update = false;
	dt = 0.005f;

	xQueuePeek(queueUWB, &uwbPos, 0);					//从队列中获取UWB数据
	xQueuePeek(queueFlow,&flow, 0);
	xQueuePeek(queueHeight, &height, 0);

	//初始状态，协方差矩阵
	X_k << uwbPos.uwbPos[0],uwbPos.uwbPos[1],uwbPos.uwbPos[2],flow.flowx,flow.flowy,height.speedZ,yaw,0.0f,0.0f,0.0f,0.0f;
	//观测变量
	y << uwbPos.uwbPos[0],uwbPos.uwbPos[1],uwbPos.uwbPos[2],flow.flowx,flow.flowy,height.speedZ;
	//转移矩阵雅可比
	Jacobin_A = MatrixXf::Identity(11, 11);
	//输出矩阵雅可比
	Jacobin_H = MatrixXf::Identity(6, 11);
	//协方差矩阵
	P_k = 1e-4f*MatrixXf::Identity(11, 11);
	//过程噪声雅可比
	Jacobin_W = MatrixXf::Identity(11, 11);
	//测量噪声雅可比
	Jacobin_V = MatrixXf::Identity(6,6);
	//过程噪声雅可比矩阵
	Q <<    0.001f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,
			0.0f,0.001f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,
			0.0f,0.0f,0.001f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,
			0.0f,0.0f,0.0f,0.001f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,
			0.0f,0.0f,0.0f,0.0f,0.001f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,
			0.0f,0.0f,0.0f,0.0f,0.0f,0.001f,0.0f,0.0f,0.0f,0.0f,0.0f,
			0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,1000.0f,0.0f,0.0f,0.0f,0.0f,
			0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.01f,0.0f,0.0f,0.0f,
			0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.01f,0.0f,0.0f,
			0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.01f,0.0f,
			0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.01f;
	//测量噪声雅可比矩阵
	R <<    0.046f,0.0f,0.0f,0.0f,0.0f,0.0f,
			0.0f,0.078f,0.0f,0.0f,0.0f,0.0f,
			0.0f,0.0f,0.108f,0.0f,0.0f,0.0f,
			0.0f,0.0f,0.0f,0.056f,0.0f,0.0f,
			0.0f,0.0f,0.0f,0.0f,0.027f,0.0f,
			0.0f,0.0f,0.0f,0.0f,0.0f,0.001f;

	source_temp << 0.0f,0.0f,0.0f;
	target_temp << 0.0f,0.0f,0.0f;
	lamda = 0;
	lamda_Ttemp = 0;
	lamda_temp = 0;
	YawCaliCnt = 0;

	for (int i=0;i<3;i++)
	{
		ekf_uwb_pos_vel.InsPos[i] = 0;
	}

	accUWB << 0.0f, 0.0f, 0.0f;
	gyrozUWB = 0.0f;
	isFirstIns = true;
	isYawCali = false;
}

void EKFUWB::EkfUwb_acc()
{
		xQueuePeek(queueAccDat, &acc, 0);					//从队列中获取加速度计数据
		xQueuePeek(queueGyrDat, &gyro, 0);
		xQueuePeek(queueAhrsEuler, &ahrsEuler, 0);
		xQueuePeek(queueUwbPosVel,&ekf_uwb_pos_vel, 0);
		//当yaw角校准后才会进入该函数，在初次进入定点模式时会记录锁尾的角度（飞机相对于地磁北的角度），减去即可得到飞机机头相对于uwb x轴的角度
		yaw = ahrsEuler.Ang[2] - ekf_uwb_pos_vel.yaw_mag_record;

		Rz << cosf(yaw),-sinf(yaw),0.0f,
			  sinf(yaw), cosf(yaw),0.0f,
				0.0f,	  0.0f,	   1.0f;

		a_k << acc.acc[0],acc.acc[1],acc.acc[2];			//机体坐标系下的加速度测量值
//		gyro_z = gyro.gyro_filter[2];
		gyro_z = yaw;

		//当前旋转矩阵
		q.w() = ahrsEuler.q[0];
		q.x() = ahrsEuler.q[1];
		q.y() = ahrsEuler.q[2];
		q.z() = ahrsEuler.q[3];
		q.normalize();
		Cb2n = q.toRotationMatrix();

		accUWB += Rz * Cb2n * a_k;
		gyrozUWB += gyro_z;
}

void EKFUWB::EkfUwb_getJacobin()
{
	Jacobin_A << 1.0f , 0.0f , 0.0f ,  dt ,  0.0f , 0.0f ,                              0.0f                           , 0.0f ,     0.0f     ,     0.0f      , 0.0f ,
				 0.0f , 1.0f , 0.0f , 0.0f ,  dt ,  0.0f ,                              0.0f                           , 0.0f ,     0.0f     ,     0.0f      , 0.0f ,
				 0.0f , 0.0f , 1.0f , 0.0f , 0.0f ,  dt ,                               0.0f                           , 0.0f ,     0.0f     ,     0.0f      , 0.0f ,
				 0.0f , 0.0f , 0.0f , 1.0f , 0.0f , 0.0f ,-dt*cosf(yaw)*(accUWB[1]+X_k[9])+sinf(yaw)*(accUWB[0]+X_k[8]), 0.0f , dt*cosf(yaw) , -dt*sinf(yaw) , 0.0f ,
				 0.0f , 0.0f , 0.0f , 0.0f , 1.0f , 0.0f , dt*cosf(yaw)*(accUWB[0]+X_k[8])-sinf(yaw)*(accUWB[1]+X_k[9]), 0.0f , dt*sinf(yaw) ,  dt*cosf(yaw) , 0.0f ,
				 0.0f , 0.0f , 0.0f , 0.0f , 0.0f , 1.0f ,                              0.0f                           , 0.0f ,     0.0f     ,     0.0f      ,  dt  ,
				 0.0f , 0.0f , 0.0f , 0.0f , 0.0f , 0.0f ,                              1.0f                           ,  dt  ,     0.0f     ,     0.0f      , 0.0f ,
				 0.0f , 0.0f , 0.0f , 0.0f , 0.0f , 0.0f ,                              0.0f                           , 0.0f ,     0.0f     ,     0.0f      , 0.0f ,
				 0.0f , 0.0f , 0.0f , 0.0f , 0.0f , 0.0f ,                              0.0f                           , 0.0f ,     0.0f     ,     0.0f      , 0.0f ,
				 0.0f , 0.0f , 0.0f , 0.0f , 0.0f , 0.0f ,                              0.0f                           , 0.0f ,     0.0f     ,     0.0f      , 0.0f ,
				 0.0f , 0.0f , 0.0f , 0.0f , 0.0f , 0.0f ,                              0.0f                           , 0.0f ,     0.0f     ,     0.0f      , 0.0f ;

	Jacobin_H << 1.0f , 0.0f , 0.0f ,     0.0f   ,    0.0f   , 0.0f ,                 0.0f               , 0.0f , 0.0f , 0.0f , 0.0f ,
				 0.0f , 1.0f , 0.0f ,     0.0f   ,    0.0f   , 0.0f ,                 0.0f               , 0.0f , 0.0f , 0.0f , 0.0f ,
				 0.0f , 0.0f , 1.0f ,     0.0f   ,    0.0f   , 0.0f ,                 0.0f               , 0.0f , 0.0f , 0.0f , 0.0f ,
				 0.0f , 0.0f , 0.0f ,  cosf(yaw) , sinf(yaw) , 0.0f ,  X_k[4]*cosf(yaw)-X_k[3]*sinf(yaw) , 0.0f , 0.0f , 0.0f , 0.0f ,
				 0.0f , 0.0f , 0.0f , -sinf(yaw) , cosf(yaw) , 0.0f , -X_k[3]*cosf(yaw)-X_k[4]*sinf(yaw) , 0.0f , 0.0f , 0.0f , 0.0f ,
				 0.0f , 0.0f , 0.0f ,     0.0f   ,    0.0f   , 1.0f ,                 0.0f               , 0.0f , 0.0f , 0.0f , 0.0f ;
}

void EKFUWB::EkfUwb_Update()
{
	xQueuePeek(queueUWB, &uwbPos, 0);					//从队列中获取UWB数据
	xQueuePeek(queueFlow, &flow, 0);
	xQueuePeek(queueHeight, &height, 0);

	if(isFirstEKF)
	{
		isFirstEKF = false;
		EkfUwb_Init();
	}

	startTimerLast = startTimer;
	getTimer_us(&startTimer);
	cycleTime_us = startTimer - startTimerLast;

	accUWB /= 5;gyrozUWB /= 5;
	u_k << accUWB[0],accUWB[1],accUWB[2]+9.788f,gyrozUWB;
	accUWB.setZero();gyrozUWB = 0.0f;

	y << uwbPos.uwbPos[0],uwbPos.uwbPos[1],   height.height,flow.flowx,flow.flowy,height.speedZ;//uwbPos.uwbPos[2],flow.flowx,flow.flowy,height.speedZ;

	//EKF迭代过程
	//Predict state
	//偏航角速度偏置值
	X_p[7] = X_k[7];
	//偏航角
	X_p[6] = X_k[6] + (u_k[3] + X_k[7]) * dt;
	//机体系转导航系旋转矩阵
	//已在EKFuWB_acc中获取
	//更新位置_0_2
	for(int i=0;i<3;i++) X_p[i] = X_k[i] + X_k[i+3] * dt;
	//更新机体线速度预测值_3_5
	temp_xold_3_5 << X_k(3),X_k(4),X_k(5);
	//_8_10
	temp_xold_8_10 << X_k(8),X_k(9),X_k(10);
	for(int i=8;i<11;i++) X_k[i] = temp_xold_8_10[i];
	temp_xpre_3_5 = temp_xold_3_5 + Rz * (accUWB + temp_xold_8_10) * dt;
	for(int i=3;i<6;i++) X_p[i] = temp_xpre_3_5[i];
	//获取雅可比矩阵
	EkfUwb_getJacobin();
	//协方差矩阵预测
	P_p = Jacobin_A * P_k * Jacobin_A.transpose() + Jacobin_W * Q * Jacobin_W.transpose();

	//更新过程
	//计算卡尔曼增益
	K = P_p * Jacobin_H.transpose() * (Jacobin_H * P_p * Jacobin_H.transpose() + Jacobin_V * R * Jacobin_V.transpose()).inverse();
	//计算协方差矩阵更新
	P_k = P_p - K * Jacobin_H * P_p;
	//更新模型输出
	//y_0_2
	for(int i=0;i<3;i++) y_k[i] = X_k[i];
	//y_3_5
	temp_yk_3_5 = Rz.transpose() * temp_xpre_3_5;
	for(int i=3;i<6;i++) y_k[i] = temp_yk_3_5[i-3];
	//更新状态变量
	X_k = X_p + K * (y - y_k);


	ekf_uwb_pos_vel.Pos[0] = X_k[0];
	ekf_uwb_pos_vel.Pos[1] = X_k[1];
	ekf_uwb_pos_vel.Pos[2] = X_k[2];

	ekf_uwb_pos_vel.Vel[0] = X_k[3];
	ekf_uwb_pos_vel.Vel[1] = X_k[4];
	ekf_uwb_pos_vel.Vel[2] = X_k[5];

	xQueueOverwrite(queueUwbPosVel,&ekf_uwb_pos_vel);

	getTimer_us(&stopTimer);
	executionTime_us = stopTimer - startTimer;
}

void EKFUWB::EkfUwb_Ins()
{
		startTimerLast1 = startTimer1;
		getTimer_us(&startTimer1);
		cycleTime_us1 = startTimer1 - startTimerLast1;

		xQueuePeek(queueUWB, &uwbPos, 0);
		xQueuePeek(queueFlow, &flow, 0);
		xQueuePeek(queueHeight, &height, 0);

		if(isFirstIns)
		{
			//记录当前位置，之后的InsPos均为基于当前点的相对位置
			for(int i=0;i<3;i++) ekf_uwb_pos_vel.InsPos[i] = uwbPos.uwbPos[i] + 0.001;
			for(int i=0;i<3;i++) ekf_uwb_pos_vel.UwbPos[i] = uwbPos.uwbPos[i] + 0.001;
			xQueueOverwrite(queueUwbPosVel,&ekf_uwb_pos_vel);
			osSemaphoreRelease(semTran);
			isFirstIns = false;
		}
		else
		{
			ekf_uwb_pos_vel.InsPos[0] += flow.flowx * dt;
			ekf_uwb_pos_vel.InsPos[1] += flow.flowy * dt;
			//z轴坐标与uwb对齐，+0.0001f防止值为0后续求取出现奇异值
			ekf_uwb_pos_vel.InsPos[2] = uwbPos.uwbPos[2] + 0.001;
			for(int i=0;i<3;i++) ekf_uwb_pos_vel.UwbPos[i] = uwbPos.uwbPos[i] + 0.001;
		}
		getTimer_us(&stopTimer1);
		executionTime_us1 = stopTimer1 - startTimer1;
}

void EKFUWB::EkfUwb_YawCali()
{
	getTimer_us(&startTimer2);

	if(YawCaliCnt > yaw_cali_window)
	{
		isYawCali = true;
		YawCaliCnt = yaw_cali_window + 1;
	}
	else if(YawCaliCnt == yaw_cali_window)
	{
		//求重心向量
		for(int i=0;i<3;i++)
		{
			target_temp(i) /= YawCaliCnt / yaw_cali_window_depart;
			source_temp(i) /= YawCaliCnt / yaw_cali_window_depart;
		}
		//求去除重心向量后的位置信息
		for(int j=0;j<YawCaliCnt / yaw_cali_window_depart;j++)
		{
			for(int i=0;i<3;i++)
			{
				target_xyz(i,j) -= target_temp(i);
				source_xyz(i,j) -= source_temp(i);
			}
		}
		//求lamda
		for(int i=0;i<3;i++)
		{
			for(int j=0;j<YawCaliCnt / yaw_cali_window_depart;j++)
			{
				lamda_Ttemp += pow(target_xyz(i,j),2);
				lamda_temp += pow(source_xyz(i,j),2);
			}
		}
		lamda = sqrt(lamda_Ttemp / lamda_temp);
		//求奇异值分解及旋转矩阵
		A = lamda * source_xyz * target_xyz.transpose();

		JacobiSVD<Eigen::MatrixXf> svd(A, ComputeThinU | ComputeThinV);
		V = svd.matrixV();U = svd.matrixU();
		Rotation = V * U.transpose();
		//求偏航角
		yaw_cali = atanf(Rotation(1,0) / Rotation(0,0));
		//限制角度为0-180
		if(yaw_cali < 0)
		{
			yaw_cali += 360.f / 180.f * M_PI;
		}
		ekf_uwb_pos_vel.yaw_cali = yaw_cali;
		xQueueOverwrite(queueUwbPosVel,&ekf_uwb_pos_vel);
	}
	else
	{
		if(YawCaliCnt % yaw_cali_window_depart == 0)
		{
			for(int i=0;i<3;i++)
			{
				//+0.0001f防止后续求取出现奇异值
				target_xyz(i,YawCaliCnt / yaw_cali_window_depart) = ekf_uwb_pos_vel.UwbPos[i] + 0.001;
				source_xyz(i,YawCaliCnt / yaw_cali_window_depart) = ekf_uwb_pos_vel.InsPos[i];
				//求重心
				target_temp(i) += target_xyz(i,YawCaliCnt / yaw_cali_window_depart);
				source_temp(i) += source_xyz(i,YawCaliCnt / yaw_cali_window_depart);
			}
			xQueueOverwrite(queueUwbPosVel,&ekf_uwb_pos_vel);
		}
	}

	YawCaliCnt ++;

	getTimer_us(&stopTimer2);
	executionTime_us2 = stopTimer2 - startTimer2;
}

bool EKFUWB::EkfUwb_isYawCali()
{
	return isYawCali;
}

EKFUWB ekfUwb((char *)"EKF_UWB_INS");

extern "C" void ekfUwb1ms_main(void *argument)
{
	for(;;)
	{
		osSemaphoreAcquire(semEkfUwb1ms,0xffffffff);
//		if(ekfUwb.EkfUwb_isYawCali()) ekfUwb.EkfUwb_acc();           //采用人工手动标定，故先去除该行
		ekfUwb.EkfUwb_acc();
	}
}

extern "C" void ekfUwb5ms_main(void *argument)
{
	for(;;)
	{
		osSemaphoreAcquire(semEkfUwb5ms,0xffffffff);
//		if(ekfUwb.EkfUwb_isYawCali())
		{
			ekfUwb.EkfUwb_Update();
		}
//		else
//		{
//			if(osSemaphoreAcquire(semEkfIns,0x00) == osOK)
//			{
//				ekfUwb.EkfUwb_Ins();
//				ekfUwb.EkfUwb_YawCali();
//			}
//		}
	}
}


